package org.nashua.tt151;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SimpleRobot;
import org.nashua.tt151.Controller.DualAction;
import org.nashua.tt151.Controller.MSKinect;
import org.nashua.tt151.RobotMap.Jaguar.Power;

/**
 * This class is sample code for driving the robot using four Jaguars, the
 * custom Controller.DualAction class that is being read from a Logitech
 * DualAction F310 (D mode) in Operator Control and the custom MSKinect class
 * in Autonomous.
 *
 * NOTE: This is using FRCSquawkSDK Version 2012.3077. When the 2013 versions
 * are released this class will be updated according.
 */
public class DriveJaguarsCustom extends SimpleRobot {
    /* Hardware */
    public static Jaguar backLeft = new Jaguar(RobotMap.Jaguar.BACK_LEFT);
    public static Jaguar backRight = new Jaguar(RobotMap.Jaguar.BACK_RIGHT);
    public static Jaguar frontLeft = new Jaguar(RobotMap.Jaguar.FRONT_LEFT);
    public static Jaguar frontRight = new Jaguar(RobotMap.Jaguar.FRONT_RIGHT);
    public static DualAction joy = new DualAction(1);  // 1st Joystick Detected
    public static MSKinect leftArm = new MSKinect(MSKinect.LEFT_ARM);
    public static MSKinect rightArm = new MSKinect(MSKinect.RIGHT_ARM);

    /* Other Variables */
    public static double power = Power.DEFAULT_POWER;  // Set to default power

    /**
     * This method is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        while (isEnabled() && isAutonomous()) {
            leftArm.queryButtons();  // Required at the beginning of each loop
            rightArm.queryButtons();  // Required at the beginning of each loop

            /* Determine Power */
            power = Power.DEFAULT_POWER; // Reset to default power
            if (leftArm.getRawButton(MSKinect.Button.HEAD_LEFT))
                power = Power.CREEP_POWER; // Set to creep while head left
            if (leftArm.getRawButton(MSKinect.Button.HEAD_RIGHT))
                power = Power.MAX_POWER;  // Set to max power while head right

            /*
             * Store axis positions to avoid accidentally setting the back and
             * the front Jaguars to 2 different values. The stored values are
             * multiplied by the power multiplier.
             */
            double left = leftArm.getY() * power;
            /*
             * Generally, the value of the right y axis needs to be reversed
             * since the motors are rotating in the opposite direction of the
             * left motors. Always check the robot as this is not always true.
             */
            double right = -rightArm.getY() * power;
            backLeft.set(left);
            frontLeft.set(left);
            backRight.set(right);
            frontRight.set(right);
        }
    }

    /**
     * This method is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        while (isEnabled() && isOperatorControl()) {
            joy.queryButtons();  // Required at the beginning of each loop

            /* Determine Power */
            power = Power.DEFAULT_POWER; // Reset to default power
            if (joy.getRawButton(DualAction.Button.LEFT_TRIGGER))
                power = Power.CREEP_POWER; // Set to creep while holding LT
            if (joy.getRawButton(DualAction.Button.RIGHT_TRIGGER))
                power = Power.MAX_POWER;  // Set to max power while holding RT

            /*
             * Store axis positions to avoid accidentally setting the back and
             * the front Jaguars to 2 different values. The stored values are
             * multiplied by the power multiplier.
             */
            // For DualAction this is the same as: double left = joy.getY();
            double left = joy.getLeftY() * power;
            /*
             * Generally, the value of the right y axis needs to be reversed
             * since the motors are rotating in the opposite direction of the
             * left motors. Always check the robot as this is not always true.
             */
            double right = -joy.getRightY() * power;
            backLeft.set(left);
            frontLeft.set(left);
            backRight.set(right);
            frontRight.set(right);
        }
    }
}